/*******************************************************************************
 *                                    ZLG
 *                         ----------------------------
 *                         innovating embedded platform
 *
 * Copyright (c) 2001-present Guangzhou ZHIYUAN Electronics Co., Ltd.
 * All rights reserved.
 *
 * Contact information:
 * web site:    https://www.zlg.cn
 *******************************************************************************/
/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "Wed_memheap2.h"

/*******************************************************************************
 * Macro operate
 ******************************************************************************/
#ifdef WED_MEMHEAP2_MEM_64
#define WED_MEMITEM_SIZE_GET(p_item)    ((uint64_t)p_item->p_item_next - (uint64_t)p_item - WED_MEMHEAP2_ITEM_SIZE)
#else
#define WED_MEMITEM_SIZE_GET(p_item)    ((uint32_t)p_item->p_item_next - (uint32_t)p_item - WED_MEMHEAP2_ITEM_SIZE)
#endif

/*******************************************************************************
 * Code
 ******************************************************************************/
/**
 * \brief 堆内存分配
 *
 * @param p_mem_heap 堆信息结构体
 * @param size       要分配的大小
 *
 * @return 成功返回分配的内存地址
 */
void *Wed_memheap2_malloc(struct Wed_memheap2 *p_mem_heap, uint32_t size){
    int                       ret;
    uint32_t                  free_size;
    struct Wed_memheap2_item *p_hdr = NULL;

    if ((p_mem_heap == NULL) || (size == 0)) {
        return NULL;
    }
    /* 对齐 */
    size = WED_ALIGN_UP(size, WED_MEMHEAP2_ALIGN_SIZE);
    /* 检查是否小于最小分配大小 */
    if (size < WED_MEMHEAP2_MINIALLOC) {
        size = WED_MEMHEAP2_MINIALLOC;
    }

    WED_DEBUG_PRINT("allocate %d on memheap 0x%x", size, (unsigned long)p_mem_heap);

    /* 有足够空间 */
    if (size < p_mem_heap->available_size) {
        free_size = 0;

        ret = Wed_mutex_lock(&p_mem_heap->lock, 5000);
        if (ret != 0) {
            return NULL;
        }

        /* 获取第一个空闲条目 */
        p_hdr = p_mem_heap->p_item_free_list->p_item_free_next;

        while ((p_hdr != p_mem_heap->p_item_free_list) && (free_size < size)) {
            /* 获取当前空闲条目的大小 */
            free_size = WED_MEMITEM_SIZE_GET(p_hdr);
            if (free_size < size) {
                /* 空间不足，移动到下一个空闲条目 */
                p_hdr = p_hdr->p_item_free_next;
            }
        }

        if (free_size >= size) {
            /* 检查是否需要被分割成两个条目 */
            if (free_size >= (size + WED_MEMHEAP2_ITEM_SIZE + WED_MEMHEAP2_MINIALLOC)){
                struct Wed_memheap2_item *p_item_new = NULL;

                /* 初始化一个新的条目 */
                p_item_new = (struct Wed_memheap2_item *)(((uint8_t *)p_hdr) + size + WED_MEMHEAP2_ITEM_SIZE);

                WED_DEBUG_PRINT("split mem block 0x%08x, next mem 0x%08x, prev mem 0x%08x, new mem 0x%08x",
                        p_hdr, p_hdr->p_item_next, p_hdr->p_item_prev, p_item_new);

                p_item_new->magic      = WED_MEMHEAP2_MAGIC;
                p_item_new->p_mem_heap = p_mem_heap;

                /* 把新的条目插入链表 */
                p_item_new->p_item_prev         = p_hdr;
                p_item_new->p_item_next         = p_hdr->p_item_next;
                p_hdr->p_item_next->p_item_prev = p_item_new;
                p_hdr->p_item_next              = p_item_new;

                /* 把找到的条目移除 */
                p_hdr->p_item_free_next->p_item_free_prev = p_hdr->p_item_free_prev;
                p_hdr->p_item_free_prev->p_item_free_next = p_hdr->p_item_free_next;
                p_hdr->p_item_free_next = NULL;
                p_hdr->p_item_free_prev = NULL;

                /* 把新的条目插入到空闲链表中 */
                p_item_new->p_item_free_next = p_mem_heap->p_item_free_list->p_item_free_next;
                p_item_new->p_item_free_prev = p_mem_heap->p_item_free_list;
                p_mem_heap->p_item_free_list->p_item_free_next->p_item_free_prev = p_item_new;
                p_mem_heap->p_item_free_list->p_item_free_next                   = p_item_new;

                WED_DEBUG_PRINT("new ptr next free 0x%08x, prev free 0x%08x",
                        (unsigned long)p_item_new->p_item_free_next, (unsigned long)p_item_new->p_item_free_prev);

                /* 重新计算可用大小 */
                p_mem_heap->available_size = p_mem_heap->available_size - size - WED_MEMHEAP2_ITEM_SIZE;
                if ((p_mem_heap->total_size - p_mem_heap->available_size) > p_mem_heap->max_used_size) {
                    p_mem_heap->max_used_size = p_mem_heap->total_size - p_mem_heap->available_size;
                }
            } else {
                /* 重新计算可用大小 */
                p_mem_heap->available_size = p_mem_heap->available_size - free_size;
                if ((p_mem_heap->total_size - p_mem_heap->available_size) > p_mem_heap->max_used_size) {
                    p_mem_heap->max_used_size = p_mem_heap->total_size - p_mem_heap->available_size;
                }
                /* 把找到的条目移除*/
                p_hdr->p_item_free_next->p_item_free_prev = p_hdr->p_item_free_prev;
                p_hdr->p_item_free_prev->p_item_free_next = p_hdr->p_item_free_next;
                p_hdr->p_item_free_next = NULL;
                p_hdr->p_item_free_prev = NULL;
            }

            /* 标记为已使用*/
            p_hdr->magic |= WED_MEMHEAP2_ITEM_USED;

            Wed_mutex_unlock(&p_mem_heap->lock);

            WED_DEBUG_PRINT("alloc memory 0x%08x, memheap 0x%08x, size %d",
                    (unsigned long)((uint8_t *)p_hdr + WED_MEMHEAP2_ITEM_SIZE),
                    (unsigned long)p_mem_heap,
                     size);

            return (void *)((uint8_t *)p_hdr + WED_MEMHEAP2_ITEM_SIZE);
        }
        Wed_mutex_unlock(&p_mem_heap->lock);
    }

    WED_ERR_PRINT("allocate memory failed");

    return NULL;
}

/**
 * \brief 堆内存地址对齐分配
 *
 * @param p_mem_heap 堆信息结构体
 * @param size       要分配的大小
 * @param align      地址对齐的数
 *
 * @return 成功返回分配的内存地址
 */
void *Wed_memheap2_align_malloc(struct Wed_memheap2 *p_mem_heap,
                                uint32_t             size,
                                uint32_t             align){
    void     *p_align;
    void     *p;
    uint32_t  align_size;

    if (IS_NOT_SECOND_PARTY(align) || (align <= 1)) {
        return NULL;
    }

    /* 对齐 4 字节对齐 */
    align = WED_ALIGN_UP(align, sizeof(void *));
    /* 获取完整的对齐大小 */
    align_size = WED_ALIGN_UP(size, sizeof(void *)) + align + sizeof(void *);

    /* 申请内存*/
    p = Wed_memheap2_malloc(p_mem_heap, align_size);
    if (p != NULL) {
#ifdef WED_MEMHEAP2_MEM_64
        /* 地址对齐 */
        p_align = (void *)WED_ALIGN_UP((uint64_t)p + sizeof(void *), align);

        /* 把实际的地址存放在返回地址的前 4 字节 */
        *((uint64_t *)((uint64_t)p_align - sizeof(void *))) = (uint64_t)p;
#else
        /* 地址对齐 */
        p_align = (void *)WED_ALIGN_UP((uint32_t)p + sizeof(void *), align);

        /* 把实际的地址存放在返回地址的前 4 字节 */
        *((uint32_t *)((uint32_t)p_align - sizeof(void *))) = (uint32_t)p;
#endif
        p = p_align;
    }
    return p;
}

/**
 * \brief 堆内存释放
 *
 * @param p_mem 要释放的内存
 */
void Wed_memheap2_free(void *p_mem){
    int                       ret;
    struct Wed_memheap2      *p_mem_heap = NULL;
    struct Wed_memheap2_item *p_hdr, *p_item_new;
    uint32_t                  insert_hdr;

    if (p_mem == NULL) {
        return;
    }

    insert_hdr = 1;
    p_item_new = NULL;
    /* 移动到条目头部 */
    p_hdr = (struct Wed_memheap2_item *)((uint8_t *)p_mem - WED_MEMHEAP2_ITEM_SIZE);

    WED_DEBUG_PRINT("free memory 0x%08x, item 0x%08x", p_mem, p_hdr);

    /* 检查魔法数 */
    if ((p_hdr->magic & WED_MEMHEAP2_MAGIC_MASK) != WED_MEMHEAP2_MAGIC) {
        WED_ERR_PRINT("memory item magic %d illegal", p_hdr->magic);
        while(1);
    }
    /* 获取堆信息 */
    p_mem_heap = p_hdr->p_mem_heap;

    ret = Wed_mutex_lock(&p_mem_heap->lock, 5000);
    if (ret != 0) {
        return;
    }

    /* 取消已使用的标志 */
    p_hdr->magic &= ~WED_MEMHEAP2_ITEM_USED;
    /* 重新调整可用大小 */
    p_mem_heap->available_size = p_mem_heap->available_size + WED_MEMITEM_SIZE_GET(p_hdr);

    /* 前面的条目不被使用，则合并 */
    if (WED_MEMHEAP2_ITEM_IS_USED(p_hdr->p_item_prev) == 0) {

        /* 调整可用大小，因为往前合并后会多一个条目结构体的大小 */
        p_mem_heap->available_size = p_mem_heap->available_size + WED_MEMHEAP2_ITEM_SIZE;

        /* 合并条目 */
        (p_hdr->p_item_prev)->p_item_next = p_hdr->p_item_next;
        (p_hdr->p_item_next)->p_item_prev = p_hdr->p_item_prev;

        /* 移到当前条目 */
        p_hdr = p_hdr->p_item_prev;

        insert_hdr = 0;
    }

    /* 后面的条目不被使用，则合并*/
    if (WED_MEMHEAP2_ITEM_IS_USED(p_hdr->p_item_next) == 0) {
        /* 调整可用大小，因为往后合并后会多一个条目结构体的大小 */
        p_mem_heap->available_size = p_mem_heap->available_size + WED_MEMHEAP2_ITEM_SIZE;

        /* 合并条目 */
        p_item_new = p_hdr->p_item_next;
        p_item_new->p_item_next->p_item_prev = p_hdr;
        p_hdr->p_item_next = p_item_new->p_item_next;

        /* 把合并的条目从空闲链表中移除 */
        p_item_new->p_item_free_next->p_item_free_prev = p_item_new->p_item_free_prev;
        p_item_new->p_item_free_prev->p_item_free_next = p_item_new->p_item_free_next;
    }
    /* 没得合并，则插入到空闲链表中 */
    if (insert_hdr) {
        p_hdr->p_item_free_next = p_mem_heap->p_item_free_list->p_item_free_next;
        p_hdr->p_item_free_prev = p_mem_heap->p_item_free_list;
        p_mem_heap->p_item_free_list->p_item_free_next->p_item_free_prev = p_hdr;
        p_mem_heap->p_item_free_list->p_item_free_next                   = p_hdr;
    }
    Wed_mutex_unlock(&p_mem_heap->lock);
}

/**
 * \brief 堆对齐内存释放
 *
 * @param p_mem 要释放的内存
 */
void Wed_memheap2_align_free(void *p_mem){
    void *p_real = NULL;

    if (p_mem != NULL) {
#ifdef WED_MEMHEAP2_MEM_64
        p_real = (void *)*(uint64_t *)((uint64_t)p_mem - sizeof(void *));
#else
        p_real = (void *)*(uint32_t *)((uint32_t)p_mem - sizeof(void *));
#endif
    }
    Wed_memheap2_free(p_real);
}

/**
 * \brief 内存堆初始化
 *
 * @param p_mem_heap   要初始化的内存堆结构体
 * @param p_start_addr 内存起始地址
 * @param size         内存大小
 *
 * @return 成功返回 0
 */
int Wed_memheap2_init(struct Wed_memheap2 *p_mem_heap,
                      void                *p_start_addr,
                      uint32_t             size){
    struct Wed_memheap2_item *p_item = NULL;

    if ((p_mem_heap == NULL) || (p_start_addr == NULL) || (size == 0)) {
        return -WED_EINVAL;
    }
    memset(p_mem_heap, 0, sizeof(struct Wed_memheap2));

    /* 初始化内存堆信息 */
    p_mem_heap->p_start_addr   = p_start_addr;
    /* 大小向下对齐舍入 */
    p_mem_heap->total_size     = WED_ALIGN_DOWN(size, WED_MEMHEAP2_ALIGN_SIZE);
    /* 真实可用大小 */
    p_mem_heap->available_size = p_mem_heap->total_size - (2 * WED_MEMHEAP2_ITEM_SIZE);
    p_mem_heap->max_used_size  = p_mem_heap->total_size - p_mem_heap->available_size;

    /* 初始化空闲条目链表头 */
    p_item                   = &(p_mem_heap->free_item_hdr);
    p_item->magic            = WED_MEMHEAP2_MAGIC;
    p_item->p_mem_heap       = p_mem_heap;
    p_item->p_item_next      = NULL;
    p_item->p_item_prev      = NULL;
    p_item->p_item_free_next = p_item;
    p_item->p_item_free_prev = p_item;

    /* 空闲条目链表头插入到空闲链表中 */
    p_mem_heap->p_item_free_list = p_item;

    /* 初始化内存为第一个条目 */
    p_item                   = (struct Wed_memheap2_item *)p_start_addr;
    p_item->magic            = WED_MEMHEAP2_MAGIC;
    p_item->p_mem_heap       = p_mem_heap;
    p_item->p_item_next      = NULL;
    p_item->p_item_prev      = NULL;
    p_item->p_item_free_next = p_item;
    p_item->p_item_free_prev = p_item;
    /* 条目指针均指向结尾块 */
    p_item->p_item_next = (struct Wed_memheap2_item *)((uint8_t *)p_item + p_mem_heap->available_size + WED_MEMHEAP2_ITEM_SIZE);
    p_item->p_item_prev = p_item->p_item_next;

    /* 插入到条目链表中 */
    p_mem_heap->p_item_list = p_item;

    /* 把第一块内存放到空闲链表中 */
    p_item->p_item_free_next = p_mem_heap->p_item_free_list->p_item_free_next;
    p_item->p_item_free_prev = p_mem_heap->p_item_free_list;

    p_mem_heap->p_item_free_list->p_item_free_next->p_item_free_prev = p_item;
    p_mem_heap->p_item_free_list->p_item_free_next                   = p_item;

    /* 移动到尾部的内存块 */
    p_item = p_item->p_item_next;
    /* 标记为被使用的状态 */
    p_item->magic            = WED_MEMHEAP2_MAGIC | WED_MEMHEAP2_ITEM_USED;
    p_item->p_mem_heap       = p_mem_heap;
    p_item->p_item_next      = (struct Wed_memheap2_item *)p_start_addr;
    p_item->p_item_prev      = (struct Wed_memheap2_item *)p_start_addr;
    /* 不属于空闲块 */
    p_item->p_item_free_next = p_item->p_item_free_prev = NULL;

    /* 初始化互斥锁 */
    return Wed_mutex_init(&p_mem_heap->lock);
}
